﻿#include "Apf.h"

APF::APF() :
    width(0), height(0), k_att(3.0), k_rep(4.0), eta(6.0), rho0(6.0), max_iterations(300) {
    // 空初始化
}


APF::~APF() {
    // 清理动态分配的内存
    for (int i = 0; i < height; ++i) {
        for (int j = 0; j < width; ++j) {
            if (map[i][j].parent) {
                delete map[i][j].parent;
            }
        }
    }
}

void APF::InitializeMap(int _width, int _height) {
    width = _width;
    height = _height;
    map.resize(height, std::vector<Node>(width));
    for (int i = 0; i < height; ++i) {
        for (int j = 0; j < width; ++j) {
            map[i][j] = {j, i, 0.0, nullptr};
        }
    }
}

void APF::SetStartEnd(Node start, Node end) {
    startNode = start;
    endNode = end;
}

void APF::AddObstacle(int x, int y) {
    obstacles.push_back({x, y});
}

std::vector<APF::Node> APF::PlanPath() {
    ComputePotentialField();
    return FindPath();
}

void APF::PrintPotentialField() {
    for (int i = 0; i < height; ++i) {
        for (int j = 0; j < width; ++j) {
            std::cout << map[i][j].potential << " ";
        }
        std::cout << std::endl;
    }
}

void APF::SetMaxIterations(int iterations) {
    max_iterations = iterations;
}

double APF::AttractivePotential(int x, int y) {
    double dist = std::sqrt(std::pow(x - endNode.x, 2) + std::pow(y - endNode.y, 2));
    return 0.5 * k_att * dist * dist;
}

double APF::RepulsivePotential(int x, int y) {
    double min_dist = std::numeric_limits<double>::max();
    for (auto& obs : obstacles) {
        double dist = std::sqrt(std::pow(x - obs.first, 2) + std::pow(y - obs.second, 2));
        if (dist < rho0 && dist < min_dist) {
            min_dist = dist;
        }
    }
    if (min_dist == std::numeric_limits<double>::max()) {
        return 0.0;
    } else {
        return 0.5 * k_rep * (1 / min_dist - 1 / rho0) * (1 / min_dist - 1 / rho0) / (eta * eta);
    }
}

void APF::ComputePotentialField() {
    for (int i = 0; i < height; ++i) {
        for (int j = 0; j < width; ++j) {
            map[i][j].potential = AttractivePotential(j, i) + RepulsivePotential(j, i);
        }
    }
}

std::vector<APF::Node> APF::FindPath() {
    std::vector<Node> path;
    Node currentNode = startNode;
    int iterations = 0;

    while (currentNode.x != endNode.x || currentNode.y != endNode.y) {
        if (iterations >= max_iterations) {
            std::cout << "Maximum number of iterations reached. Path not found." << std::endl;
            break;
        }
        Node nextNode = FindNextNode(currentNode);
        path.push_back(nextNode);
        currentNode = nextNode;
        iterations++;
    }

    if (currentNode.x == endNode.x && currentNode.y == endNode.y) {
        path.push_back(endNode);
    }

    return path;
}

APF::Node APF::FindNextNode(Node node) {
    double minPotential = std::numeric_limits<double>::max();
    Node bestNode = node;
    for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
            if ((dx == 0 && dy == 0) || node.x + dx < 0 || node.x + dx >= width || node.y + dy < 0 || node.y + dy >= height) {
                continue;
            }
            double potential = map[node.y + dy][node.x + dx].potential;
            if (potential < minPotential) {
                minPotential = potential;
                bestNode = {node.x + dx, node.y + dy, potential, &node};
            }
        }
    }
    return bestNode;
}
